#include <rtthread.h>
#include "./Modbus/RTU/Master/modbusrtu_master.h"
#include "./Move_Control.h"
#include "../parameter.h"
#include "./Modbus/modbus_slave_core.h"
#include "./Move_Control.h"
#include "./Location.h"
#include "../Drivers/PGV.h"

#define LOG_TAG "hmi"       /* 模块TAG */
#define LOG_LVL LOG_LVL_DBG /* 静态过滤级别为调试级 */
#include <ulog.h>           /* 必须放在宏定义下面 */

static void main_thread(void *parameter);

static struct rt_thread _thread;
ALIGN(RT_ALIGN_SIZE)
static char _thread_stack[2048];

union HMI_Float {
    float fdata; /* 浮点数 */
    struct
    {
        short sdata_low;
        short sdata_high;
    };
    long ldata;
} ALIGN(RT_ALIGN_SIZE); /* 定义了HMI使用的浮点数类型 */

union HMI_Int {
    int idata; /* 整型 */
    struct
    {
        short sdata_low;
        short sdata_high;
    };
} ALIGN(RT_ALIGN_SIZE); /* 定义了HMI使用的双字类型 */

rt_inline float swap_float_hw(float value) { return value; } /* 转换HMI使用的浮点数和MCU使用的浮点数 */
rt_inline int swap_int_hw(int value) { return value; }       /* 转换HMI使用的整型和MCU使用的整型 */

static char slave_addr = 0;             /* 触摸屏从机地址 */
static unsigned char power_on = 0;      /* 上电标志,线圈地址0 */
static unsigned short page_index = 0;   /* 当前页,寄存器地址0 */
static unsigned short move_control = 0; /* 运动控制,寄存器地址1 */

static struct
{
    short module_init_flag;   /* 运动模块初始化标志 */
    short move_state;         /* 运动状态 */
    short dm_flag;            /* 二维码读取标志 */
    short avoid_state;        /* 避障检测 */
    union HMI_Int wrong_code; /* 错误代码 */
    union HMI_Int dm_tag;     /* 二维码TAG号 */
    union HMI_Int dm_x;       /* 二维码x值 */
    union HMI_Int dm_y;       /* 二维码y值 */
    union HMI_Float vx;       /* vx(mm/s) */
    union HMI_Float vy;       /* vy(mm/s) */
    union HMI_Float angular;  /* w(°) */
    union HMI_Float x;        /* x(mm) */
    union HMI_Float y;        /* y(mm) */
    union HMI_Float theta;    /* theta(°) */
} page1;                      /* 页面1 */

static struct
{
    unsigned short slave_addr;          /* 从机地址 */
    unsigned short wtd_enable_flag;     /* 看门狗使能标志 */
    union HMI_Float obstacle_velocity;  /* 有障碍物时的低速度(mm/s) */
    union HMI_Float dec_emergency_time; /* 急停时的减速度时间(s) */
    union HMI_Float default_acc;        /* 默认加速度时间(s)，从零速到最高速所需时间 */
    union HMI_Float default_dec;        /* 默认减速度时间(s)，从最高速到零速所需时间 */
    union HMI_Float motor_max_rpm;      /* 电机最高转速(n/min) */
    union HMI_Float motor_min_rpm;      /* 电机最低转速(n/min) */
} page2, page2_old;                     /* 页面2 */

static struct
{
    unsigned short avoid_slave_addr;    /* 避障检测从机地址 */
    unsigned short indicate_slave_addr; /* 指示设备从机地址 */
    union HMI_Float pgv_x;              /* PGV传感器x轴偏置(mm) */
    union HMI_Float pgv_y;              /* PGV传感器y轴偏置(mm) */
    union HMI_Float pgv_theta_offset;   /* PGV传感器theta偏置(°) */
} page3, page3_old;                     /* 页面3 */

static struct
{
    union HMI_Float wheel_diameter;        /* 车轮直径(mm) */
    union HMI_Float motor_reduction_ratio; /* 减速比 */
    union HMI_Float distance_x;            /* 两轮之间x轴距离 */
    union HMI_Float distance_y;            /* 两轮之间y轴距离 */
    union HMI_Float overall_x;             /* 车宽 */
    union HMI_Float overall_y;             /* 车长 */
    union HMI_Float pgv_theta;             /* PGV传感器安装角度(°) */
} page4, page4_old;                        /* 页面4 */

static void Update_Page1(void); /* 更新页面1 */
static void Update_Page2(void); /* 更新页面2 */
static void Update_Page3(void); /* 更新页面3 */
static void Update_Page4(void); /* 更新页面4 */

void hmi_thread_init(char addr)
{
    slave_addr = addr;
    rt_err_t rt_result;
    rt_result = rt_thread_init(&_thread, "hmi thread", main_thread, RT_NULL, _thread_stack, sizeof(_thread_stack), 10, 2);
    if (rt_result == RT_EOK)
    {
        rt_thread_startup(&_thread); /* 启动线程 */
    }
    else
    {
        rt_kprintf("hmi thread error\n");
    }
}

void main_thread(void *parameter)
{
    Modbus_Error_Code mb_result;
    while (1)
    {
        mb_result = MR_Read_Coils(slave_addr, 0, 1, &power_on, 200);
        if ((Modbus_Master_OK == mb_result) && (power_on))
            break;
    }
    LOG_I("hmi power on."); /* 触摸屏上电 */
    rt_thread_delay(100);   /* 延时等待 */

    Update_Page1();
    rt_thread_delay(100);
    {
        /* 更新页面2的数据 */
        page2.slave_addr = agv_parameter->agv_address;
        page2.obstacle_velocity.fdata = swap_float_hw(agv_parameter->obstacle_velocity);
        page2.dec_emergency_time.fdata = swap_float_hw(agv_parameter->dec_emergency_time);
        page2.default_acc.fdata = swap_float_hw(agv_parameter->default_acc);
        page2.default_dec.fdata = swap_float_hw(agv_parameter->default_dec);
        page2.motor_max_rpm.fdata = swap_float_hw(agv_parameter->motor_max_rpm);
        page2.motor_min_rpm.fdata = swap_float_hw(agv_parameter->motor_min_rpm);
        page2.wtd_enable_flag = agv_parameter->wtd_enable_flag;

        while (1)
        {
            mb_result = MR_Write_Multiple_Holding_Registers(slave_addr, 200, sizeof(page2) / sizeof(unsigned short), (unsigned short *)&page2, RT_WAITING_FOREVER);
            if (Modbus_Master_OK == mb_result)
                break;
        }
    }
    rt_thread_delay(100); /* 延时等待 */
    {
        /* 更新页面3数据 */
        page3.avoid_slave_addr = agv_parameter->avoid_slave;
        page3.indicate_slave_addr = agv_parameter->indicate_slave;
        page3.pgv_x.fdata = swap_float_hw(agv_parameter->pgv_x);
        page3.pgv_y.fdata = swap_float_hw(agv_parameter->pgv_y);
        page3.pgv_theta_offset.fdata = swap_float_hw(agv_parameter->pgv_theta_offset);

        while (1)
        {
            mb_result = MR_Write_Multiple_Holding_Registers(slave_addr, 300, sizeof(page3) / sizeof(unsigned short), (unsigned short *)&page3, RT_WAITING_FOREVER);
            if (Modbus_Master_OK == mb_result)
                break;
        }
    }
    rt_thread_delay(100); /* 延时等待 */
    {
        /* 更新页面4数据 */
        page4.wheel_diameter.fdata = swap_float_hw(agv_parameter->wheel_diameter);
        page4.motor_reduction_ratio.fdata = swap_float_hw(agv_parameter->motor_reduction_ratio);
        page4.distance_x.fdata = swap_float_hw(agv_parameter->distance_x);
        page4.distance_y.fdata = swap_float_hw(agv_parameter->distance_y);
        page4.overall_x.fdata = swap_float_hw(agv_parameter->overall_x);
        page4.overall_y.fdata = swap_float_hw(agv_parameter->overall_y);
        page4.pgv_theta.fdata = swap_float_hw(agv_parameter->pgv_theta);

        while (1)
        {
            mb_result = MR_Write_Multiple_Holding_Registers(slave_addr, 400, sizeof(page4) / sizeof(unsigned short), (unsigned short *)&page4, RT_WAITING_FOREVER);
            if (Modbus_Master_OK == mb_result)
                break;
        }
    }
    LOG_I("hmi inited."); /* 触摸屏数据初始化完成 */

    while (1)
    {
        rt_thread_delay(50); /* 100ms读取一次 */
        mb_result = MR_Read_Holding_Registers(slave_addr, 0, 1, &page_index, RT_WAITING_FOREVER);
        if (Modbus_Master_OK != mb_result)
            continue;

        switch (page_index)
        {
        case 1:
            MR_Read_Holding_Registers(slave_addr, 1, 1, &move_control, RT_WAITING_FOREVER);
            if (0 != move_control)
            {
                Write_Move_Control_Event((enum Move_Control_Event_Enum)move_control);
                move_control = 0;
                MR_Writer_Holding_Register(slave_addr, 1, move_control, RT_WAITING_FOREVER);
            }

            Update_Page1();
            break;
        case 2:
            Update_Page2();
            break;
        case 3:
            Update_Page3();
            break;
        case 4:
            Update_Page4();
            break;
        default:
            break;
        }
    }
}

void Update_Page1(void)
{
    int temp;
    page1.module_init_flag = Return_Init_Flag();
    page1.move_state = (short)Read_AGV_Move_State();
    temp = Read_Data_Matrix();
    if (-1 == temp)
    {
        page1.dm_flag = swap_int_hw(0);
    }
    else
    {
        page1.dm_flag = swap_int_hw(1);
        page1.dm_tag.idata = swap_int_hw(temp);
        int x, y;
        PGV_Cal_X_Y(&x, &y, temp);
        page1.dm_x.idata = swap_int_hw(x * 100);
        page1.dm_y.idata = swap_int_hw(y * 100);
    }
    page1.avoid_state = Return_Avoid_Check();
    page1.wrong_code.idata = (int)Read_Wrong_Code();

    struct Location_Coor_Struct coor;
    struct Location_Velocity_Struct v;
    Read_Location_Coor(&coor);
    Read_Location_Velocity(&v);

    page1.vx.fdata = swap_float_hw(v.vx);
    page1.vy.fdata = swap_float_hw(v.vy);
    page1.angular.fdata = swap_float_hw(v.w);
    page1.x.fdata = swap_float_hw(coor.x_coor);
    page1.y.fdata = swap_float_hw(coor.y_coor);
    page1.theta.fdata = swap_float_hw(coor.theta);

    MR_Write_Multiple_Holding_Registers(slave_addr, 100, sizeof(page1) / sizeof(unsigned short), (unsigned short *)&page1, RT_WAITING_FOREVER);
}

void Update_Page2(void)
{
    union parameter_data_union value;
    rt_memcpy(&page2_old, &page2, sizeof(page2));                                                                      /* 保存旧数据 */
    MR_Read_Holding_Registers(slave_addr, 200, sizeof(page2) / sizeof(unsigned short), (unsigned short *)&page2, 500); /* 读寄存器 */
    /* 判断变量是否改动 */
    if (page2.slave_addr != page2_old.slave_addr)
    {
        value.cvalue = page2.slave_addr;
        Updata_Parameter(Parameter_AGV_Address, value);
    }
    if (page2.wtd_enable_flag != page2_old.wtd_enable_flag)
    {
        value.cvalue = page2.wtd_enable_flag;
        Updata_Parameter(Parameter_WTD_Enable, value);
    }
    if (page2.obstacle_velocity.ldata != page2_old.obstacle_velocity.ldata)
    {
        value.fvalue = swap_float_hw(page2.obstacle_velocity.fdata);
        Updata_Parameter(Parameter_Obstacle_Velocity, value);
    }
    if (page2.dec_emergency_time.ldata != page2_old.dec_emergency_time.ldata)
    {
        value.fvalue = swap_float_hw(page2.dec_emergency_time.fdata);
        Updata_Parameter(Parameter_Dec_Emergency_Time, value);
    }
    if (page2.default_acc.ldata != page2_old.default_acc.ldata)
    {
        value.fvalue = swap_float_hw(page2.default_acc.fdata);
        Updata_Parameter(Parameter_Default_Acc, value);
    }
    if (page2.default_dec.ldata != page2_old.default_dec.ldata)
    {
        value.fvalue = swap_float_hw(page2.default_dec.fdata);
        Updata_Parameter(Parameter_Default_Dec, value);
    }
    if (page2.motor_max_rpm.ldata != page2_old.motor_max_rpm.ldata)
    {
        value.fvalue = swap_float_hw(page2.motor_max_rpm.fdata);
        Updata_Parameter(Parameter_Motor_Max_RPM, value);
    }
    if (page2.motor_min_rpm.ldata != page2_old.motor_min_rpm.ldata)
    {
        value.fvalue = swap_float_hw(page2.motor_min_rpm.fdata);
        Updata_Parameter(Parameter_Moter_Min_RPM, value);
    }
}
void Update_Page3(void)
{
    union parameter_data_union value;
    rt_memcpy(&page3_old, &page3, sizeof(page3));                                                                      /* 保存旧数据 */
    MR_Read_Holding_Registers(slave_addr, 300, sizeof(page3) / sizeof(unsigned short), (unsigned short *)&page3, 500); /* 读寄存器 */
    /* 判断变量是否改动 */
    if (page3.avoid_slave_addr != page3_old.avoid_slave_addr)
    {
        value.cvalue = page3.avoid_slave_addr;
        Updata_Parameter(Parameter_Avoid_Slave, value);
    }
    if (page3.indicate_slave_addr != page3_old.indicate_slave_addr)
    {
        value.cvalue = page3.indicate_slave_addr;
        Updata_Parameter(Parameter_Indicate_Slave, value);
    }
    if (page3.pgv_x.ldata != page3_old.pgv_x.ldata)
    {
        value.fvalue = swap_float_hw(page3.pgv_x.fdata);
        Updata_Parameter(Parameter_PGV_X, value);
    }
    if (page3.pgv_y.ldata != page3_old.pgv_y.ldata)
    {
        value.fvalue = swap_float_hw(page3.pgv_y.fdata);
        Updata_Parameter(Parameter_PGV_Y, value);
    }
    if (page3.pgv_theta_offset.ldata != page3_old.pgv_theta_offset.ldata)
    {
        value.fvalue = swap_float_hw(page3.pgv_theta_offset.fdata);
        Updata_Parameter(Parameter_PGV_Theta_Offset, value);
    }
}
void Update_Page4(void)
{
    union parameter_data_union value;
    rt_memcpy(&page4_old, &page4, sizeof(page4));                                                                      /* 保存旧数据 */
    MR_Read_Holding_Registers(slave_addr, 400, sizeof(page4) / sizeof(unsigned short), (unsigned short *)&page4, 500); /* 读寄存器 */
    /* 判断变量是否改动 */
    if (page4.wheel_diameter.ldata != page4_old.wheel_diameter.ldata)
    {
        value.fvalue = swap_float_hw(page4.wheel_diameter.fdata);
        LOG_I("wheel diameter %.2f", value.fvalue);
        Updata_Parameter(Parameter_Wheel_Diameter, value);
    }
    if (page4.motor_reduction_ratio.ldata != page4_old.motor_reduction_ratio.ldata)
    {
        value.fvalue = swap_float_hw(page4.motor_reduction_ratio.fdata);
        LOG_I("motor reduction %.2f", value.fvalue);
        Updata_Parameter(Parameter_Motor_Reduction_Ratio, value);
    }
    if (page4.distance_x.ldata != page4_old.distance_x.ldata)
    {
        value.fvalue = swap_float_hw(page4.distance_x.fdata);
        LOG_I("distance x %.2f", value.fvalue);
        Updata_Parameter(Parameter_Distance_X, value);
    }
    if (page4.distance_y.ldata != page4_old.distance_y.ldata)
    {
        value.fvalue = swap_float_hw(page4.distance_y.fdata);
        LOG_I("distance y %.2f", value.fvalue);
        Updata_Parameter(Parameter_Disyance_Y, value);
    }
    if (page4.overall_x.ldata != page4_old.overall_x.ldata)
    {
        value.fvalue = swap_float_hw(page4.overall_x.fdata);
        LOG_I("overall x %.2f", value.fvalue);
        Updata_Parameter(Parameter_Overall_X, value);
    }
    if (page4.overall_y.ldata != page4_old.overall_y.ldata)
    {
        value.fvalue = swap_float_hw(page4.overall_y.fdata);
        LOG_I("overall y %.2f", value.fvalue);
        Updata_Parameter(Parameter_Overall_Y, value);
    }
    if (page4.pgv_theta.ldata != page4_old.pgv_theta.ldata)
    {
        value.fvalue = swap_float_hw(page4.pgv_theta.fdata);
        LOG_I("pgv theta %.2f", value.fvalue);
        Updata_Parameter(Parameter_PGV_Theta, value);
    }
}
